#include "CSerialPort.h"
#include "plane.h"
#include "getAmmo.h"
#include <netinet/in.h>
#include<sys/ioctl.h>
#include <thread>

#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <vector>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/impl/point_types.hpp>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/console/time.h>
#include <pcl/features/normal_3d.h>

#include <iostream>
#include <string.h>

using namespace std;
typedef pcl::PointXYZ PointT;


char data[8];

short x, z;
float y_min;

pcl::PassThrough<PointT> pass;                         
pcl::VoxelGrid<PointT> sor;

pcl::EuclideanClusterExtraction<PointT> ec;

pcl::PassThrough<PointT> pass_plane;                         
pcl::VoxelGrid<PointT> sor_plane;
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; 
pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::ExtractIndices<PointT> extract;
		pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
		pcl::PointCloud<PointT>::Ptr cloud_fil(new pcl::PointCloud<PointT>);
		pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
pcl::console::TicToc tt;

		
		short flag=0;
		
		
pcl::PointCloud<pcl::PointXYZ>::Ptr points_to_pcl(const rs2::points& points)
{
    auto sp = points.get_profile().as<rs2::video_stream_profile>();
    cloud->width = sp.width();
    cloud->height = sp.height();
    cloud->is_dense = true;
    cloud->points.resize(points.size());
	cloud_fil->points.resize(0);
   
    auto ptr = points.get_vertices();
	
    for (auto& p : cloud->points)
    {
		
		p.x = ptr->x;
        p.y = ptr->y;
        p.z = ptr->z;
		
       
	   
		cloud_fil->points.push_back(p);
	

		 ptr++;
    }
	
    return cloud_fil;
}											   // ����ص�����cloud_cb_,��ȡ������ʱ�����ݽ��д���
int main()
{
	while (true)
	{
		rs2::context devices_list;
		//cout<<"0 ";
		flag=(devices_list.query_all_sensors()).size();
		//cout<<"1 ";
		while(flag==0)
			{
		//cout<<"2 ";
				rs2::context devices_list;
				flag=(devices_list.query_all_sensors()).size();
				usleep(1000);
		//cout<<"3 ";
			}		
		rs2::pointcloud pc;
		rs2::points points;
		rs2::pipeline pipe;
		pipe.start();

		//cout<<"4 ";
		std::string title;
		int n=0;
		
			pcl::visualization::CloudViewer viewer("123456");
		while(true)
		{
			tt.tic();
			rs2::context devices_list;
			if((devices_list.query_all_sensors()).size()==0) break;

			auto frames = pipe.wait_for_frames();
			auto depth = frames.get_depth_frame();
		    points = pc.calculate(depth);

				cloud_filtered = points_to_pcl(points);        
				
		pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());

		pcl::search::KdTree<PointT>::Ptr tmp_tree(new pcl::search::KdTree<PointT>());


		pcl::PointCloud<PointT>::Ptr cloud_filtered1(new pcl::PointCloud<PointT>);
		pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
		pcl::PointCloud<PointT>::Ptr cloud_filtered_plane(new pcl::PointCloud<PointT>);
		pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients);
		pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices);
		pcl::PointCloud<pcl::PointXYZ>::Ptr all_cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT>());

		tt.tic();

		data[0] = 1;
		int times = 0;
		short pre_data[10][3];//0 x;  1 z;   2 size; 

							  // Build a passthrough filter to remove spurious NaNs

		pass_plane.setInputCloud(cloud_filtered);
		pass_plane.setFilterFieldName("y");
		pass_plane.setFilterLimits(-0.35, 0.05);
		pass_plane.filter(*cloud_filtered_plane);
		
		sor_plane.setInputCloud(cloud_filtered_plane);
		sor_plane.setLeafSize(0.015f, 0.015f, 0.015f);
		sor_plane.filter(*cloud_filtered_plane);

		ne.setSearchMethod(tmp_tree);
		ne.setInputCloud(cloud_filtered_plane);
		ne.setKSearch(50);
		ne.compute(*cloud_normals);

		seg.setOptimizeCoefficients(true);
		seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
		seg.setNormalDistanceWeight(0.1);
		seg.setMethodType(pcl::SAC_RANSAC);
		seg.setMaxIterations(100);
		seg.setDistanceThreshold(0.03);
		seg.setInputCloud(cloud_filtered_plane);
		seg.setInputNormals(cloud_normals);
		// Obtain the plane inliers and coefficients
		seg.segment(*inliers_plane, *coefficients_plane);
		
		extract.setInputCloud(cloud_filtered_plane);
		extract.setIndices(inliers_plane);
		extract.setNegative(false);

		// Write the planar inliers to disk
		extract.filter(*cloud_plane);

		float a, b, c, d;
		short angle=0, distance=0;
		if (coefficients_plane->values.size() > 0)
		{
			all_cloud_cluster->operator+=(*cloud_plane);
			a = coefficients_plane->values[0];
			b = coefficients_plane->values[1];
			c = coefficients_plane->values[2];
			d = coefficients_plane->values[3];
			angle = (atan2(c, a) * 180 / 3.14 - 90)*100;
			distance = (abs(d) / sqrt(a*a + b*b + c*c))*1000;
			cout << "角度: " << angle/100;
			cout << "    距离" << distance/ 10<<endl;
			data[5]=2;
		}



		sor.setInputCloud(cloud);
		sor.setLeafSize(0.004f, 0.008f, 0.005f);
		sor.filter(*cloud_filtered);

		pass.setInputCloud(cloud_filtered);
		pass.setFilterFieldName("y");
		pass.setFilterLimits(-0.15, 0.15);
		pass.filter(*cloud_filtered);

		//std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl;

		tree->setInputCloud(cloud_filtered);

		std::vector<pcl::PointIndices> cluster_indices;
		ec.setClusterTolerance(0.012); // 1.3cm
		ec.setMinClusterSize(500);
		ec.setMaxClusterSize(1300);
		ec.setSearchMethod(tree);
		ec.setInputCloud(cloud_filtered);
		ec.extract(cluster_indices);

		all_cloud_cluster->resize(cloud_filtered->size());
		for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
		{
			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
			for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
				cloud_cluster->points.push_back(cloud_filtered->points[*pit]);
			cloud_cluster->width = cloud_cluster->points.size();
			cloud_cluster->height = 1;
			cloud_cluster->is_dense = true;

			pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
			feature_extractor.setInputCloud(cloud_cluster);
			feature_extractor.compute();

			pcl::PointXYZ min_point_AABB;
			pcl::PointXYZ max_point_AABB;
			feature_extractor.getAABB(min_point_AABB, max_point_AABB);

			if (z > 0.4)y_min = 0.295;
			else y_min = 0.28;
			if (((max_point_AABB.x - min_point_AABB.x)>0.06) && ((max_point_AABB.x - min_point_AABB.x)<0.115) && ((max_point_AABB.z - min_point_AABB.z)<0.11) && ((max_point_AABB.y - min_point_AABB.y)>y_min))
			{
				all_cloud_cluster->operator+=(*cloud_cluster);
				pre_data[times][0] = ((max_point_AABB.x + min_point_AABB.x) / 2) * 1000;
				pre_data[times][1] = min_point_AABB.z * 1000;
				pre_data[times][2] = cloud_cluster->size();
				times++;
				std::cout << max_point_AABB.x - min_point_AABB.x << "  " << max_point_AABB.z - min_point_AABB.z << "  " << max_point_AABB.y - min_point_AABB.y << "  " << ((max_point_AABB.x + min_point_AABB.x) / 2) * 100 << "  " << min_point_AABB.z * 100 << "        size: " << cloud_cluster->points.size() << std::endl;
			}
			//std::cout << "                       size: " << cloud_cluster->points.size() << " data points." << std::endl;

		}
		if (times == 0)
		{
			x = z = 0;
			data[0] = 0;
		}
		else if (times == 1)
		{
			x = pre_data[0][0];
			z = pre_data[0][1];
		}
		else
		{
			short temp_x, temp_z, temp_size, temp_min_size, max_n, min_n;
			temp_x = temp_z = temp_size = temp_min_size = max_n = min_n = 0;
			for (int i = 0; i < times; i++)
			{
				if (pre_data[i][2] > temp_size)
				{
					temp_min_size = temp_size;
					min_n = max_n;
					temp_size = pre_data[i][2];
					max_n = i;
				}
			}
			if (temp_size - temp_min_size > 100)
			{
				x = pre_data[max_n][0];
				z = pre_data[max_n][1];
			}
			else
			{
				if (abs(pre_data[max_n][0]) < abs(pre_data[min_n][0]))
				{
					x = pre_data[max_n][0];
					z = pre_data[max_n][1];
				}
				else
				{
					x = pre_data[min_n][0];
					z = pre_data[min_n][1];
				}
			}
		}
		if(data[5]==2&&data[0]==1)data[0]=1;
		else data[0]=0;
		std::cout << x/10 << "  " << z/10<<endl;

		data[1] = x;
		data[2] = (x >> 8);
		data[3] = z;
		data[4] = (z >> 8);
		data[5] = angle;
		data[6] = (angle >> 8);
		data[7] = 0x5A;		
		std::cerr << "    Done: " << tt.toc() << " ms\n\n";

		if (all_cloud_cluster->size()>0)viewer.showCloud(all_cloud_cluster);
                                                                                                 
		}
	}
}
